#ifndef EXECUTOR_H
#define EXECUTOR_H

#include <cmath>
#include <iostream>
#include <libplayerc++/playerc++.h>
#include "pause.h"

class Executor {

  const ompl::geometric::PathGeometric & path;
  PlayerCc::PlayerClient & robot;
  PlayerCc::Position2dProxy & pp;
  PlayerCc::Position2dProxy & gocon;

  bool at_position(player_pose2d pose) {
    // TODO: use spaceinformation?
    return (fabs(pose.px - pp.GetXPos()) + fabs(pose.py - pp.GetYPos())) < 0.05;
  }

public:
  Executor(const ompl::geometric::PathGeometric & path,
    PlayerCc::PlayerClient & robot,
    PlayerCc::Position2dProxy & pp,
    PlayerCc::Position2dProxy & gocon)
  : path(path), robot(robot), pp(pp), gocon(gocon) {
  }

  // execute_plan()
  void execute() {
      std::cout << "start to execute" << std::endl;
      std::size_t stateCount = path.getStateCount();

      for (std::size_t i = 0; i < stateCount; i++) {
        const ompl::base::SE2StateSpace::StateType * state = path.getState(i)->as<ompl::base::SE2StateSpace::StateType>();

        player_pose2d waypoint;
        waypoint.px = state->getX();
        waypoint.py = state->getY();
        waypoint.pa = state->getYaw();

        if (!at_position(waypoint)) {
          // if not at position, send the goto command
          std::cout << "  Going to (" << waypoint.px << ", " << waypoint.py << ")\n";
          gocon.GoTo(waypoint);

          // and wait till reach
          do {
            util::pause(1);
            robot.Read();
          } while (!at_position(waypoint));
        }
      }
   }
};

#endif
